#include "ax25.h"

#include <vector>

// ===== 内部工具函数声明 =====
static void encodeAddressField(const String &call, bool last, std::vector<uint8_t> &out);
static String decodeAddressField(const uint8_t *addr);

// ===== CRC/FCS 计算 =====
uint16_t ax25_calcFcs(const uint8_t *data, size_t len) {
  uint16_t crc = 0xFFFF;
  const uint16_t poly = 0x8408; // x^16 + x^12 + x^5 + 1（倒序多项式）
  for (size_t i = 0; i < len; ++i) {
    crc ^= data[i];
    for (int b = 0; b < 8; ++b) {
      if (crc & 0x0001) {
        crc = (crc >> 1) ^ poly;
      } else {
        crc >>= 1;
      }
    }
  }
  return crc ^ 0xFFFF;
}

bool ax25_verifyFcs(const std::vector<uint8_t> &frame, uint16_t recvFcs) {
  if (frame.empty()) return false;
  uint16_t calc = ax25_calcFcs(frame.data(), frame.size());
  return calc == recvFcs;
}

// ===== 帧编码 =====
bool ax25_buildUIFrame(const String &dest,
                       const String &src,
                       const std::vector<String> &digis,
                       const std::vector<uint8_t> &info,
                       std::vector<uint8_t> &out) {
  out.clear();
  if (dest.length() == 0 || src.length() == 0) return false;

  std::vector<String> path = digis;
  const size_t addrCount = 2 + path.size();
  out.reserve(14 + path.size() * 7 + info.size() + 2);

  // 目的 / 源 / 中继 地址字段
  for (size_t i = 0; i < addrCount; ++i) {
    bool last = (i == addrCount - 1);
    if (i == 0) {
      encodeAddressField(dest, last && path.empty(), out);
    } else if (i == 1) {
      encodeAddressField(src, last && path.empty(), out);
    } else {
      encodeAddressField(path[i - 2], last, out);
    }
  }

  // Control & PID（UI 帧固定取值）
  out.push_back(0x03);
  out.push_back(0xF0);

  // Info 字段
  out.insert(out.end(), info.begin(), info.end());

  // FCS 低字节在前
  uint16_t fcs = ax25_calcFcs(out.data(), out.size());
  out.push_back((uint8_t)(fcs & 0xFF));
  out.push_back((uint8_t)(fcs >> 8));

  return true;
}

// ===== 帧解析 =====
bool ax25_parseFrame(const std::vector<uint8_t> &frame, Ax25Frame &out) {
  out = Ax25Frame();
  if (frame.size() < 16) return false; // 至少包含目的和源地址 + 控制/PID

  size_t offset = 0;
  std::vector<String> addresses;
  while (offset + 7 <= frame.size()) {
    addresses.push_back(decodeAddressField(&frame[offset]));
    bool hasEndBit = frame[offset + 6] & 0x01;
    offset += 7;
    if (hasEndBit) break;
  }

  if (addresses.size() < 2) return false;
  if (offset + 2 > frame.size()) return false;

  out.destination = addresses[0];
  out.source = addresses[1];
  for (size_t i = 2; i < addresses.size(); ++i) {
    out.digipeaters.push_back(addresses[i]);
  }

  out.control = frame[offset++];
  out.pid = frame[offset++];
  if (offset > frame.size()) return false;

  out.info.assign(frame.begin() + offset, frame.end());
  out.fcsOk = true;
  return true;
}

// ===== 内部工具实现 =====
static void encodeAddressField(const String &call, bool last, std::vector<uint8_t> &out) {
  String temp = call;
  temp.trim();
  temp.toUpperCase();

  int dash = temp.indexOf('-');
  int ssid = 0;
  if (dash >= 0) {
    ssid = temp.substring(dash + 1).toInt();
    temp = temp.substring(0, dash);
  }
  while (temp.length() < 6) temp += ' ';
  if (temp.length() > 6) temp = temp.substring(0, 6);

  for (int i = 0; i < 6; ++i) {
    char c = temp.charAt(i);
    if (c < ' ') c = ' ';
    out.push_back(((uint8_t)c) << 1);
  }

  uint8_t ssidField = 0x60 | ((ssid & 0x0F) << 1);
  if (last) ssidField |= 0x01;
  out.push_back(ssidField);
}

static String decodeAddressField(const uint8_t *addr) {
  char call[7];
  for (int i = 0; i < 6; ++i) {
    call[i] = (char)(addr[i] >> 1);
  }
  call[6] = '\0';
  String result = String(call);
  result.trim();

  int ssid = (addr[6] >> 1) & 0x0F;
  if (ssid > 0) {
    result += "-";
    result += String(ssid);
  }
  return result;
}

